/*
Arm Tracker and Glove Demo by Noah Zerkin (noazark AT gmail.com) 2009
Released under GNU GPLv3.0 http://www.opensource.org/licenses/gpl-3.0.html
*/

void armR(int s){
  a2hH[s] = (sin(.5*elbowAngle[s])*forearmLength);         //H = Distance from Shoulder to Hand
  a2hR[s] = (cos(.5*elbowAngle[s])*forearmLength);         //R = Distance of Elbow from midpoint of H
  a2hZFF[s] = cos(HALF_PI+yTilt[s])*a2hR[s];                  //What I call the Z Flex Factor
  a2hAngle[s] = (PI+zTilt[s])-(atan(a2hZFF[s]/a2hH[s]));         //Angle between H and horizon
  a2hHx[s] = -(cos(a2hAngle[s])*2*a2hH[s]);                   //Hand X
  a2hHy[s] = sin(a2hAngle[s])*2*a2hH[s];                      //Hand Y
  a2hF[s] = sqrt(sq(a2hZFF[s])+sq(a2hH[s]));
  a2hEx[s] = cos(zTilt[s]-(2*(atan(a2hZFF[s]/a2hH[s]))))*a2hF[s];   //Elbow X
  a2hEy[s] = sin(a2hAngle[s]-(atan(a2hZFF[s]/a2hH[s])))*a2hF[s];    //Elbow Y
  a2hEz[s] = (sin(HALF_PI+yTilt[s])*-a2hR[s]);                //Elbow Z
  //  line(0,0,0,a2hHx,a2hHy,0);
  strokeWeight(10);
  line(0,0,0,a2hEx[s],a2hEy[s],a2hEz[s]);
  //line(shoulder[s][0],shoulder[s][1],shoulder[s][2],a2hEx[s],a2hEy[s],a2hEz[s]);
  line(a2hEx[s],a2hEy[s],a2hEz[s],a2hHx[s],a2hHy[s],0);
  sphereDetail(3);
    pushMatrix();
    for(int i=0; i<=10; i++) {
      pushMatrix();
      translate(lerp(0, a2hEx[s], i/10.0), lerp(0, a2hEy[s], i/10.0), lerp(0, a2hEz[s], i/10.0));
    sphere(10);
    popMatrix();
    }
    for(int i=0; i<=9; i++) {
      pushMatrix();
      translate(lerp(a2hEx[s], a2hHx[s], i/10.0), lerp(a2hEy[0], a2hHy[s], i/10.0), lerp(a2hEz[s], 0, i/10.0));
    sphere(7);
    popMatrix();
    }
    popMatrix();
    sphereDetail(20);
  drawHandR(s);
  //  obj[0][1] = modelX(a2hHx,a2hHy,0);
  //  obj[0][2] = modelY(a2hHx,a2hHy,0);
  //  obj[0][3] = modelZ(a2hHx,a2hHy,0);
}

void armL(int s){
  a2hH[s] = (sin(.5*elbowAngle[s])*forearmLength);         //H = Distance from Shoulder to Hand
  a2hR[s] = (cos(.5*elbowAngle[s])*forearmLength);         //R = Distance of Elbow from midpoint of H
  a2hZFF[s] = cos(HALF_PI+yTilt[s])*a2hR[s];                  //What I call the Z Flex Factor
  a2hAngle[s] = (PI+zTilt[s])-(atan(a2hZFF[s]/a2hH[s]));         //Angle between H and horizon
  a2hHx[s] = -(cos(a2hAngle[s])*2*a2hH[s]);                   //Hand X
  a2hHy[s] = sin(a2hAngle[s])*2*a2hH[s];                      //Hand Y
  a2hF[s] = sqrt(sq(a2hZFF[s])+sq(a2hH[s]));
  a2hEx[s] = cos(zTilt[s]-(2*(atan(a2hZFF[s]/a2hH[s]))))*a2hF[s];   //Elbow X
  a2hEy[s] = sin(a2hAngle[s]-(atan(a2hZFF[s]/a2hH[s])))*a2hF[s];    //Elbow Y
  a2hEz[s] = -(sin(HALF_PI+yTilt[s])*-a2hR[s]);                //Elbow Z
  //  line(0,0,0,a2hHx,a2hHy,0);

    
  strokeWeight(3);
  drawHandL(s);
  //  obj[0][1] = modelX(a2hHx,a2hHy,0);
  //  obj[0][2] = modelY(a2hHx,a2hHy,0);
  //  obj[0][3] = modelZ(a2hHx,a2hHy,0);
}
